/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "dragcontrol.h"

#include "trajectoryserver.h"
#include "MacroDefine/GeneralDefine.h"
#include "filteringalgorithm.h"

DragControl::DragControl(QString configFilePathIn,TrajectoryServer *trajServerIn,
                         RobotCommonParameter *roboParameterIn)
{
    configFilePath=configFilePathIn;
    trajServer=trajServerIn;
    roboParameter=roboParameterIn;
    jointNum=roboParameter->jointNum;
    robotId=roboParameterIn->robotId;
    deltaTime=roboParameterIn->deltaTime;
    dragStatus.resize(roboParameter->jointNum);

    for(int i=0;i<roboParameter->jointNum;i++)
    {
        dragStatus[i]=ENUM_DRAG_STOPPED;
    }

    isInitialOkStatus=false;


    dragParaList.resize(6);
    for(int i=0;i<6;i++)
    {
//        dragParaList[i].controlKp=0.1;
//        dragParaList[i].thresholdTorque=1;
        FilteringAlgorithm *tmpFilter=new FilteringAlgorithm;
        filterAlgo.append(tmpFilter);

        FilteringAlgorithm *tmpTorqueFilter=new FilteringAlgorithm;
        torquefilterAlgo.append(tmpTorqueFilter);

    }

    int tmpReturn = readConfigFile(configFilePath);
    if(1==tmpReturn)
    {
        isInitialOkStatus=true;
    }


}

int DragControl::setCollisionWarnTorque(int axisIndex, double torqueIn)
{
    if(axisIndex>=0 && axisIndex<warnTorqueBiasList.size())
    {
        warnTorqueBiasList[axisIndex]=torqueIn;
        qDebug()<<"setCollisionWarnTorque"<<axisIndex<<torqueIn;
    }
    return 1;
}

int DragControl::setCollisionDetectRatio(double ratioIn)
{
    collisionDetectRatio=ratioIn;
    return 1;
}

bool DragControl::isInitialOk()
{
    return isInitialOkStatus;
}

int DragControl::setDragPara(int axisIndex, DragPara paraIn)
{
    if(axisIndex>=0 && axisIndex<dragParaList.size())
    {
        dragParaList[axisIndex]=paraIn;
        qDebug()<<"setDragPara"<<axisIndex<<paraIn.controlKp<<paraIn.thresholdTorque;
    }
    return 1;

}


int DragControl::dragLoopRealTime()
{
    if(false==isUseThisModule)
    {
        return 0;
    }
    QVector<double> jointsPositionOut, jointsVelOut,jointsAccOut, jointsTorqueOut,idealTorqueList;
    trajServer->getJointFeedback(jointsPositionOut, jointsVelOut,jointsAccOut, jointsTorqueOut);
    trajServer->getIdealTorque(idealTorqueList);
    if(0==jointsPositionOut.size())
    {
        return 0;
    }
    switch (roboParameter->robotType)
    {
    case ENUM_UR_6AXIS:
    {
        if(6>jointsPositionOut.size())
            {
                return 0;
            }
    //        for(int i=0;i<6;i++)
    //        {

    //            double torqueCommandOut;
    //            int tmpkey=dragLoopOneAxis(jointsTorqueOut[i],idealTorqueList[i],dragParaList[i].controlKp
    //                            ,dragParaList[i].thresholdTorque,torqueCommandOut);

    //            trajServer->moveOneStepByTorque(torqueCommandOut,i);
    //            if(1==tmpkey)
    //            {
    //                qDebug()<<"axis"<<i<<"torqueFeedback"<<jointsTorqueOut[i]<<"torqueCommandOut"<<torqueCommandOut
    //                       <<"idealTorque"<<idealTorqueList[i]<<"controlKp"<<dragParaList[i].controlKp
    //                          <<"thresholdTorque"<<dragParaList[i].thresholdTorque;
    //            }
    //        }
            for(int i=0;i<6;i++)
            {
                double maxError=0.3;
//                int i=5;
                double positionErrorOut;
                int tmpkey=dragLoopOneAxis_soft(jointsTorqueOut[i],idealTorqueList[i],dragParaList[i].controlKp
                                ,dragParaList[i].thresholdTorque,positionErrorOut);
                if(maxError<positionErrorOut)
                {
                    positionErrorOut=maxError;
                }
                else if(-maxError>positionErrorOut)
                {
                    positionErrorOut=-maxError;
                }
                positionErrorOut=filterAlgo[i]->arithmeticMeanfilter_low(positionErrorOut,
                                                                         dragParaList[i].filterCount);
                trajServer->moveOneStepByPosition_plus(positionErrorOut,i);
                if(1==tmpkey)
                {
//                    qDebug()<<"axis"<<i<<"torqueFeedback"<<jointsTorqueOut[i]<<"positionErrorOut"<<positionErrorOut
//                           <<"idealTorque"<<idealTorqueList[i]<<"controlKp"<<dragParaList[i].controlKp
//                              <<"thresholdTorque"<<dragParaList[i].thresholdTorque;
                }
            }
            break;
    }

    default:
    {
        qDebug()<<"dragLoopRealTime,unsupport  robotType"<<roboParameter->robotType;
    }

    }
//    trajServer->resetAll_exceptStatus();
    return 1;
}

int DragControl::dragLoopCommon()
{

   // dragLoopCommonSingleAxis(3);
//    dragLoopCommonSingleAxis(2);

//    for(int i=0;i<roboParameter->jointNum;i++)
//    {
//        dragLoopCommonSingleAxis(i);
//    }

    return 1;
}

int DragControl::dragLoopCommonSingleAxis(int jointIndex)
{

    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;
    targetPoint.jointFlag=JOINT_FLAG_JOINT;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(4);
    targetPoint.positionValue.resize(jointNum);


        QVector<double> jointsPosition, jointsVel,jointAcc, jointTorque;
        trajServer->getFeedbackJoint(jointsPosition, jointsVel,jointAcc, jointTorque);
        if(ENUM_DRAG_STOPPED==dragStatus[jointIndex])
        {
            if(thresholdStartValue[jointIndex]<jointTorque[jointIndex]-idealValue[jointIndex])
            {
                targetPoint.positionValue[jointIndex]=jointsPosition[jointIndex]-moveLength[jointIndex];
                targetPoint.extPositionValue.resize(0);
                tmpMovePara.moveType = ENUM_COMMAND_SINGLE_AXIS_SPLINE;
                tmpMovePara.velocityUnit = ENUM_RATIO;
                tmpMovePara.velocity = 0.5;
                tmpMovePara.pointType = ENUM_NORMAL_POINT;
                tmpMovePara.targetPoint = targetPoint;
                tmpMovePara.index = 3;
                tmpMovePara.cntRatio = 0;
                tmpMovePara.acc = 0;
                tmpMovePara.lineNumber = ENUM_LINE_DRAG;
                tmpMovePara.singleAxisFlag=jointIndex;
                motionParaList.append(tmpMovePara);
                double acceleration=fabs(jointTorque[jointIndex]-idealValue[jointIndex]-thresholdStartValue[jointIndex])
                        /fabs(thresholdStartValue[jointIndex]);
                if(acceleration<5)
                {
//                    acceleration=acceleration*6;
                    acceleration=acceleration*10;
                }
                else
                {
                    acceleration=5*10;
                }
                trajServer->clearTraj_byAxisId(jointIndex,true);
                trajServer->addTrajectoryCommand(motionParaList);
                //trajServer->setTimeAxisScale(0.5,0.2,jointIndex);
                trajServer->setTimeAxisScaleByTrap(0.2,jointIndex,acceleration);//加速度和速度不能太快，否则受力误判（因为没有考虑加速度力）
                //trajServer->setTimeAxisScaleBySpline(0.5,jointIndex);
                dragStatus[jointIndex]=ENUM_DRAG_REVERSE_RUN;

                qDebug()<<" ENUM_DRAG_REVERSE_RUN thresholdStartValue[jointIndex]<jointTorque[jointIndex]"<<jointTorque[jointIndex];



            }
            else if(-thresholdStartValue[jointIndex]>jointTorque[jointIndex]-idealValue[jointIndex])
            {
                targetPoint.positionValue[jointIndex]=jointsPosition[jointIndex]+moveLength[jointIndex];
                targetPoint.extPositionValue.resize(0);
                tmpMovePara.moveType = ENUM_COMMAND_SINGLE_AXIS_SPLINE;
                tmpMovePara.velocityUnit = ENUM_RATIO;
                tmpMovePara.velocity = 0.5;
                tmpMovePara.pointType = ENUM_NORMAL_POINT;
                tmpMovePara.targetPoint = targetPoint;
                tmpMovePara.index = 3;
                tmpMovePara.cntRatio = 0;
                tmpMovePara.acc = 0;
                tmpMovePara.lineNumber = ENUM_LINE_DRAG;
                tmpMovePara.singleAxisFlag=jointIndex;
                motionParaList.append(tmpMovePara);
                trajServer->clearTraj_byAxisId(jointIndex,true);
                trajServer->addTrajectoryCommand(motionParaList);

                double acceleration=fabs(jointTorque[jointIndex]-idealValue[jointIndex]-thresholdStartValue[jointIndex])
                        /fabs(thresholdStartValue[jointIndex]);
                if(acceleration<5)
                {
                    //acceleration=acceleration*6;
                    acceleration=acceleration*10;
                }
                else
                {
                    acceleration=5*10;
                }
                //trajServer->setTimeAxisScale(0.5,0.2,jointIndex);
                trajServer->setTimeAxisScaleByTrap(0.2,jointIndex,acceleration);
                //trajServer->setTimeAxisScaleBySpline(0.5,jointIndex);
                dragStatus[jointIndex]=ENUM_DRAG_FORWARD_RUN;
                trajServer->setBasicStatus(ENUM_TRAJECTORY_RUN);
                qDebug()<<"ENUM_DRAG_FORWARD_RUN -thresholdStartValue[jointIndex]>jointTorque[jointIndex]"<<jointTorque[jointIndex];

            }

//            trajServer->calculateTrajectoryPath();


        }
        if( ENUM_DRAG_REVERSE_RUN==dragStatus[jointIndex])
        {
            if(thresholdRunValue[jointIndex]>jointTorque[jointIndex]-idealValue[jointIndex])
            {
                //trajServer->setTimeAxisScale(0,0.2,jointIndex);
                trajServer->setTimeAxisScaleByTrap(0,jointIndex,3);
                 //trajServer->setTimeAxisScaleBySpline(0,jointIndex);
                dragStatus[jointIndex]=ENUM_DRAG_REVERSE_STOPPING;
                qDebug()<<"ENUM_DRAG_REVERSE_STOPPING "<<jointTorque[jointIndex];

            }
        }

        if(ENUM_DRAG_FORWARD_RUN==dragStatus[jointIndex] )
        {
            if(-thresholdRunValue[jointIndex]<jointTorque[jointIndex]-idealValue[jointIndex])
            {
                //trajServer->setTimeAxisScale(0,0.2,jointIndex);
                trajServer->setTimeAxisScaleByTrap(0,jointIndex,3);
                //trajServer->setTimeAxisScaleBySpline(0,jointIndex);
                dragStatus[jointIndex]=ENUM_DRAG_FORWARD_STOPPING;
                qDebug()<<"ENUM_DRAG_FORWARD_STOPPING "<<jointTorque[jointIndex];


            }
        }

        if(ENUM_DRAG_REVERSE_STOPPING==dragStatus[jointIndex])
        {
            if(ENUM_TIME_AXIS_NORMAL==trajServer->getTimeAxisStatus(jointIndex))
            {
               dragStatus[jointIndex]=ENUM_DRAG_STOPPED;
               qDebug()<<"ENUM_DRAG_STOPPED  "<<jointTorque[jointIndex];
            }
            else if(thresholdRunValue[jointIndex]<jointTorque[jointIndex]-idealValue[jointIndex])
            {
                dragStatus[jointIndex]=ENUM_DRAG_REVERSE_RUN;
                //trajServer->setTimeAxisScale(0.5,0.2,jointIndex);
                trajServer->setTimeAxisScaleByTrap(0.2,jointIndex,3);
                //trajServer->setTimeAxisScaleBySpline(0.5,jointIndex);
                qDebug()<<"ENUM_DRAG_REVERSE_STOPPING -> ENUM_DRAG_REVERSE_RUN  "<<jointTorque[jointIndex];
            }

        }

        if(ENUM_DRAG_FORWARD_STOPPING==dragStatus[jointIndex])
        {
            if(ENUM_TIME_AXIS_NORMAL==trajServer->getTimeAxisStatus(jointIndex))
            {
               dragStatus[jointIndex]=ENUM_DRAG_STOPPED;
            }
            else if(-thresholdRunValue[jointIndex]>jointTorque[jointIndex]-idealValue[jointIndex])
            {
                dragStatus[jointIndex]=ENUM_DRAG_FORWARD_RUN;
                //trajServer->setTimeAxisScale(0.5,0.2,jointIndex);
                trajServer->setTimeAxisScaleByTrap(0.2,jointIndex,3);
                //trajServer->setTimeAxisScaleBySpline(0.5,jointIndex);
                qDebug()<<"ENUM_DRAG_FORWARD_STOPPING -> ENUM_DRAG_FORWARD_RUN  "<<jointTorque[jointIndex];
            }

        }
        return 1;
}

int DragControl::readConfigFile(QString filePathIn)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, DragControl::readConfigFile open file error！";
       return -1;
    }


    QDomNode  inNode;

    inNode;

    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "isUseThisModule", ok);
    if( !ok )
    {
        qDebug()<<"error, collisionDetectRatio node error！";
        return -1;
    }
    xmlParser.readXml( inNode, isUseThisModule  );
    if(false==isUseThisModule)
    {
        qDebug()<<"不用拖拽示教模块！";
        return 1;
    }

    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "collisionDetectRatio", ok);
    if( !ok )
    {
        qDebug()<<"error, collisionDetectRatio node error！";
        return -1;
    }
    xmlParser.readXml( inNode, collisionDetectRatio  );

    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "count", ok);
    if( !ok )
    {
        qDebug()<<"error, count node error！";
        return -1;
    }

    int axisCount;
    xmlParser.readXml( inNode, axisCount  );
    if( !ok )
    {
        qDebug()<<"error, countread node error！";
        return -1;
    }
    dragParaList.clear();
    DragPara tmppara;
    warnTorqueBiasList.clear();
    double tmpWarnTorque;
    for(int i=0;i<axisCount;i++)
    {
         QDomNode tmpNode =  xmlParser.findSubNode(xmlParser.getRootDomElement(), QString("axis%1").arg(i), ok);
         if( !ok  )
         {
            qDebug()<<"DragControl::readRobotInfo====读内部轴参数失败: axis "<<i;
            return -1;
         }

         xmlParser.readXml( xmlParser.findSubNode(tmpNode, "controlKp", ok), tmppara.controlKp  );
         if( !ok )
         {
             qDebug()<<"error, controlKp node error！";
             return -1;
         }

         xmlParser.readXml( xmlParser.findSubNode(tmpNode, "thresholdTorque", ok), tmppara.thresholdTorque  );
         if( !ok )
         {
             qDebug()<<"error, thresholdTorque node error！";
             return -1;
         }

         xmlParser.readXml( xmlParser.findSubNode(tmpNode, "maxPositionError", ok), tmppara.maxPositionError  );
         if( !ok )
         {
             qDebug()<<"error, maxPositionError node error！";
             return -1;
         }

         xmlParser.readXml( xmlParser.findSubNode(tmpNode, "filterCount", ok), tmppara.filterCount  );
         if( !ok )
         {
             qDebug()<<"error, filterCount node error！";
             return -1;
         }

         dragParaList.append(tmppara);

         //warnTorqueBias
         xmlParser.readXml( xmlParser.findSubNode(tmpNode, "warnTorqueBias", ok), tmpWarnTorque  );
         if( !ok )
         {
             qDebug()<<"error, warnTorqueBias node error！";
             return -1;
         }

         warnTorqueBiasList.append(tmpWarnTorque);

    }

    xmlParser.closeXml( );

    return 1;
}

int DragControl::dragOn()
{
    if(false==isUseThisModule)
    {
        return 0;
    }
    for(int i=0;i<filterAlgo.size();i++)
    {
        filterAlgo[i]->resetAll();
    }
   trajServer->servoOn();
   sleep(1);
   trajServer->setCheckMode(E_CHECK_MODE_T1);
   trajServer->setBasicStatus(ENUM_TRAJECTORY_RUN);
   switch (roboParameter->robotType)
   {
   case ENUM_UR_6AXIS:
//       for(int i=0;i<2;i++)
//       {
//            trajServer->setMotorControlMode(i,E_MOTOR_CONTROL_CST_MODE);
//       }
       break;
   default:
       break;
   }

   return 1;
}

int DragControl::dragOff()
{
    if(false==isUseThisModule)
    {
        return 0;
    }
    trajServer->resetAll_exceptStatus();
    switch (roboParameter->robotType)
    {
    case ENUM_UR_6AXIS:
//        for(int i=0;i<6;i++)
//        {
//             trajServer->setMotorControlMode(i,E_MOTOR_CONTROL_CSP_MODE);
//        }
        break;
    default:
        break;
    }

    return 1;
}

int DragControl::collisionDetectLoop()
{
    if(false==isUseThisModule)
    {
        return -1;
    }
    QVector<double> jointsPositionOut, jointsVelOut,jointsAccOut, jointsTorqueOut,idealTorqueList;
    trajServer->getJointFeedback(jointsPositionOut, jointsVelOut,jointsAccOut, jointsTorqueOut);
    trajServer->getIdealTorque(idealTorqueList);
    if(0==jointsPositionOut.size())
    {
        return -1;
    }
    switch (roboParameter->robotType)
    {
    case ENUM_UR_6AXIS:
    {
        if(6>jointsPositionOut.size())
            {
                return -1;
            }

            for(int i=0;i<6;i++)
            {
//                int i=5;
                double filterTorque=torquefilterAlgo[i]->arithmeticMeanfilter_low(jointsTorqueOut[i],5);
                int tmpkey=collisionDetectLoop_oneAxis(filterTorque,idealTorqueList[i]
                                                       ,warnTorqueBiasList[i]);

                if(0==tmpkey)
                {
//                    qDebug()<<"collision axis"<<i<<"torqueFeedback"<<jointsTorqueOut[i]<<"filterTorque"<<filterTorque
//                           <<"idealTorque"<<idealTorqueList[i]
//                              <<"warnTorqueBiasList"<<warnTorqueBiasList[i];
                    return i;
                }

            }
            break;
    }

    default:
    {
        qDebug()<<"collisionDetectLoop,unsupport  robotType"<<roboParameter->robotType;
    }

    }
//    trajServer->resetAll_exceptStatus();
    return -1;
}

int DragControl::collisionDetectLoop_oneAxis(double torqueFeedback, double idealTorque,
                                             double warnTorqueBias)
{
    //    qDebug()<<"torqueFeedback"<<torqueFeedback<<"idealTorque"<<idealTorque<<"controlKp"<<controlKp
    //              <<"thresholdTorque"<<thresholdTorque;
        double torqueBias=fabs(torqueFeedback-idealTorque);
        if(torqueBias>warnTorqueBias*collisionDetectRatio)
        {
    //        qDebug()<<"torqueBias<thresholdTorque  torqueCommandOut"<<torqueCommandOut;
            return 0;
        }

    //    qDebug()<<"torqueCommandOut"<<torqueCommandOut;
        return 1;
}

int DragControl::dragLoopOneAxis_soft(double torqueFeedback, double idealTorque, double controlKp,
                                      double thresholdTorque, double &positionErrorOut)
{
    //    qDebug()<<"torqueFeedback"<<torqueFeedback<<"idealTorque"<<idealTorque<<"controlKp"<<controlKp
    //              <<"thresholdTorque"<<thresholdTorque;
        double torqueBias=fabs(torqueFeedback-idealTorque);
        if(torqueBias<thresholdTorque)
        {
            positionErrorOut=0;
    //        qDebug()<<"torqueBias<thresholdTorque  torqueCommandOut"<<torqueCommandOut;
            return 0;
        }
        positionErrorOut=controlKp*(idealTorque-torqueFeedback);
    //    qDebug()<<"torqueCommandOut"<<torqueCommandOut;
        return 1;
}

int DragControl::dragLoopOneAxis(double torqueFeedback,double idealTorque, double controlKp,
                                 double thresholdTorque, double &torqueCommandOut)
{
//    qDebug()<<"torqueFeedback"<<torqueFeedback<<"idealTorque"<<idealTorque<<"controlKp"<<controlKp
//              <<"thresholdTorque"<<thresholdTorque;
    double torqueBias=fabs(torqueFeedback-idealTorque);
    if(torqueBias<thresholdTorque)
    {
        torqueCommandOut=idealTorque;
//        qDebug()<<"torqueBias<thresholdTorque  torqueCommandOut"<<torqueCommandOut;
        return 0;
    }
    torqueCommandOut=controlKp*(idealTorque-torqueFeedback);
//    qDebug()<<"torqueCommandOut"<<torqueCommandOut;
    return 1;
}
